import pyrealsense2 as rs
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import skimage.io

pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30)
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)

pf = pipeline.start(config)

print(f"device: {pf.get_device()}")
print(f"depth_sensor: {pf.get_device().first_depth_sensor()}")
print(f"depth_scale: {pf.get_device().first_depth_sensor().get_depth_scale()}")
print(f"streams: {pf.get_streams()}")

# 将深度图对齐到RGB
align_to = rs.stream.color
align = rs.align(align_to)
print(align)

# 获取一帧数据
frame = pipeline.wait_for_frames()
print(f"data: {frame.data}")
print(f"frame_number: {frame.frame_number}")
print(f"frame_timestamp_domain: {frame.frame_timestamp_domain}")
print(f"profile: {frame.profile}")
print(f"timestamp: {frame.timestamp}")

data_size = frame.get_data_size()
print(data_size)

color_rs = frame.get_color_frame()
color_image = np.asanyarray(color_rs.get_data())

depth_rs = frame.get_depth_frame()
depth_image = np.asanyarray(depth_rs.get_data())

pc = rs.pointcloud()
pc.map_to(color_rs)
points_rs = pc.calculate(depth_rs)

skimage.io.imsave("depth.png", depth_image)
skimage.io.imsave("color.png", color_image)